/*
 * motormotion.cxx
 *
 *  Created on: Sep 7, 2013
 *      Author: jacob
 */
#include <stdio.h>
#include <time.h>
#include <string.h>
#include <cstdlib>
#include <errno.h>
#include <sys/socket.h>
#include <resolv.h>
#include <arpa/inet.h>
#include <unistd.h>

#define MY_PORT		9923//9999
#define MAXBUF		1024
#define UDP			1

#define USPI 150
#define BIAS 300
#define CLIFF 3

#define ABS(x) (x > 0 ? x : -x)

int step(int argc, char **argv)
{


    //printf("Hello World!\n");

    int sockfd;
    struct sockaddr_in self;
   	char buffer[MAXBUF];
   	int ret = 0;

#ifdef UDP
   	/*---Create datagram socket---*/
   	if ( (sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0 )
   	    {
   		perror("Socket");
   		exit(errno);
   	}
#else
    /*---Create streaming socket---*/
    if ( (sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0 )
    {
    	perror("Socket");
    	exit(errno);
    }
#endif

    /*---Initialize address/port structure---*/
    //bzero(&self, sizeof(self));
    memset(&self,0,sizeof(self));
    self.sin_family = AF_INET;
    self.sin_port = htons(MY_PORT);
    self.sin_addr.s_addr = htonl(INADDR_ANY);

    /*---Assign a port number to the socket---*/
    if ( bind(sockfd, (struct sockaddr*)&self, sizeof(self)) != 0 )
    {
    	perror("socket--bind");
    	exit(errno);
    }

#ifndef UDP
    /*---Make it a "listening socket"---*/
    //check to see if this is the problem with recv stopping working
    if ( listen(sockfd, 20) != 0 )
    {
    	perror("socket--listen");
    	exit(errno);
    }
#endif

    printf("Connected\n");

    //make connection loop
    int clientfd;
    struct sockaddr_in client_addr;
    socklen_t addrlen=sizeof(client_addr);

#ifndef UDP
    /*---accept a connection (creating a data pipe)---*/
    clientfd = accept(sockfd, (struct sockaddr*)&client_addr, &addrlen);
    printf("%s:%d connected\n", inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));
#endif


	while (1)
	{

#ifndef UDP
		ret = recv(clientfd, buffer, MAXBUF, 0);
#else
		ret = recvfrom(sockfd,buffer,MAXBUF,0,(struct sockaddr *)&client_addr,&addrlen);
#endif
		//printf("recv ret: %d\n", ret);
		if(ret <= 0)
		{
			continue;
		}
		printf("Received\n");
		//printf("%s\n",buffer);

		if(0 == strncmp("falling",buffer,7))
		{
			printf("Falling\n");
			// TODO ADD CODE FOR HAS FALLEN
		}
		else if(0 == strncmp("backward",buffer,8))
		{

		}
		else if(0 == strncmp("right",buffer,5))
		{

			//usleep(500000); //wait, and then store the new positions of the motors

		}
		else if(0 == strncmp("left",buffer,4))
		{

		}
		else if(0 == strncmp("stop",buffer,4))
		{

		}
		else if(0 == strncmp("speed",buffer,5))
		{
			//parse speed
			//speed = 1000*atoi(&buffer[5]);
			//printf("Speed = %d\n",speed);
		}
		else if(0 == strncmp("exit",buffer,4))
		{
			/*
			isBackwardCorrecting = false;
			isForwardCorrecting = false;
			break;
			*/
		}

		printf("%s\n",buffer);

		//zero out buffer
		memset(buffer, 0, MAXBUF);

		/*---Close data connection---*/
		//close(clientfd);

		//usleep(100000); //TODO: remove this
	}
#ifndef UDP
	close(clientfd);
#endif
	/*---Clean up (should never get here!)---*/
	close(sockfd);

	//CTextLcd::Release();
	return 0;
}
